% SourceFitDipoleBz.m -- Auxiliary MATLAB script for fitting a magnetic dipole model
%    to Bz data using mixed linear and nonlinear least squares. This script
%    is called by QDMDipoleFittingBatch.m
%  References: - R.R. Fu, E.A. Lima, M.W.R. Volk, R. Trubko (2020) "High-sensitivity
%  moment magnetometry with the quantum diamond microscope,"  Geochemistry,
%  Geophysics, Geosystems.
%              - E. A. Lima and B. P. Weiss (2016) "Ultra-high Sensitivity
%  Moment Magnetometry of Geological Samples Using Magnetic Microscopy,"
%  Geochemistry, Geophysics, Geosystems, 17, doi:10.1002/2016GC006487.
%
% -----------------------------------------------------------------------------------
%   Matlab code by Eduardo A. Lima - Copyright (C) 2012-2020 MIT Paleomagnetism Lab
% -----------------------------------------------------------------------------------

function  [resid, Bzmodel, M] = SourceFitDipole(P,x,y,BzExp,display,method);

const = 1.0*10^-7; %field in T

x0=P(1);   % parameters representing the coordinates of the dipole fed by
y0=P(2);   % nonlinear optimization algorithm
h=P(3);

%build geometry matrix
A=zeros(numel(BzExp),3);
delx = x-x0;
dely = y-y0;
l5i = (delx.^2+dely.^2+h^2).^-2.5;

A(:,1)=3*const*h*delx(:).*l5i(:);
A(:,2)=3*const*h*dely(:).*l5i(:);
A(:,3)=const*(2*h*h*ones(numel(BzExp),1)-delx(:).^2-dely(:).^2).*l5i(:);

Scl=diag(1./sqrt(sum(A.^2)));   %Matrix scaling using 2-norm of columns
Ascl=A*Scl;                     %Scale columns of A
Mu=Ascl\BzExp(:);               %Compute solution to least-squares problem
                                %via MATLAB's mldivide approach

M=Scl*Mu;                       %Rescale solution

Bz=A*M;                         %Compute model field map
Bz=reshape(Bz,size(BzExp));     %Reshape field map as matrix

residual=Bz-BzExp;              %Compute residuals map

if display                      %Show experimental map, model map, and
                                %residuals map, if selected
    figure(2);
    subplot(2,2,2);
    imagesc(Bz);
    axis xy, axis equal, axis tight
    caxis([-1 1]*max(abs(caxis)));
    colorbar
    subplot(2,2,3);
    imagesc(residual);
    axis xy, axis equal, axis tight
    caxis([-1 1]*max(abs(caxis)));
    colorbar
    drawnow;
    %pause(1)                   %uncomment this line to slow code down
                                %for visualization purposes
end

if method
    resid=norm(residual,2);     %Non-linear least squares requires norm of residuals
else
    resid=residual;             %Simplex method requires map of residuals
end

Bzmodel=Bz;
end
